/*
 * Copyright (C) Tsingmicro Intelligent Technology Co., Limited
 */
#include <stdlib.h>
#include <stdbool.h>
#include <assert.h>
#include <stdio.h>
#include <aos/kernel.h>
#include "if_v.h"
#include "aos/kv.h"
#include "vfs.h"
#include <fcntl.h>
#include <sys/time.h>
#include <cx_facerecog.h>
#include <math.h>
#include "userFaceSample.h"
#include "ff_mgr.h"
#include "uart_packet.h"
#include "cli_api.h"
#if defined(AOS_COMP_LITTLEFS) || defined(AOS_COMP_FATFS)
#define AOS_CONFIG_FF_USE_FS  
#else
#define AOS_CONFIG_FF_USE_KV  
#endif

#ifndef FIXED_LENGTH_SWITCH
#define FIXED_LENGTH_SWITCH  0
#endif

/***************************   使用流程   *****************************
1.初始化
	user_face_initial_sample()		        //初始化，开机调用一次即可

2. 收到用户命令后调用
	user_multi_angle_reg_sample_run()	    //触发多角度算法（用户发不同的命令，函数参数不同）
		
3.算法回调
	user_multi_angle_result_check_sample() ->user_multi_angle_reg_result_check_sample()		//判断该角度是否通过
                                                            |
                                                            |
                                                            v
		                                        multi_angle_result_send_user_sample()		//结果、状态信息返回给用户

4.用户判断所有角度注册都成功
	user_face_manage_ff_add()			        //保存用户id及特征信息
***************************   使用流程结束   ***************************/

////////////////////////////global///////////////////////////////////
////////////////////////////global///////////////////////////////////
////////////////////////////global///////////////////////////////////

static int user_ff_add_to_fs(uint32_t id, void *pFeature, char flag);
static int user_ff_delete_from_fs(uint32_t id, char flag);
static int user_ff_all_delete_from_fs(char *buf, uint32_t len);
static int user_ff_load_from_fs(void); 
static int create_user_ff_dir_and_files(void);
extern void resetAck(void);
extern void faceResetAck(void);
extern void multiAngleRegAck(uint8_t cmd, int result, uint16_t id, uint8_t direction);
extern int user_face_manage_ff_get_usr_id(void);
extern void multiAngleRegNote(int result, int16_t left,int16_t top,int16_t right,int16_t bottom,int16_t yaw,int16_t pitch,int16_t roll);
extern void multiAngleRecogAck(int result, uint16_t id, uint8_t *name, uint8_t admin, uint8_t eyeStatus);
extern void multiAngleRecogNote(int result, int16_t left,int16_t top,int16_t right,int16_t bottom,int16_t yaw,int16_t pitch,int16_t roll);

int user_face_manage_ff_set_usr_id(int id);

extern data_flow_func_control_t g_ifv_flow_cfg;


extern uint8_t g_multiAngleRegister;

uint8_t *g_userFaceFeature = NULL;
uint8_t *g_userfaceFeatureTemp = NULL;

user_face_algo_sample_t g_userFaceAlgoSample;
user_face_manage_sample_t g_userFaceManageSample;

uint8_t g_userFfMap[USER_FACE_FEATURE_MAX_NUM] = {0};
////////////////////////////basic interface/////////////////////////
////////////////////////////basic interface/////////////////////////
////////////////////////////basic interface/////////////////////////
void multi_angle_reg_run(void)
{
    g_multiAngleRegister = 1;

    cx_facerecog_user_enroll(0, 0);
}

void multi_angle_recog_run(void)
{
    cx_facerecog_user_verify(0);
}

int32_t multiAngleFacePositionCheck(TsmFaceBox *nirBox)
{
    if (NULL == nirBox)
        return FACE_STATE_NOFACE;

    if ((nirBox->xmax - nirBox->xmin) < FACE_POSITION_BOUNDARY_FAR)
        return FACE_STATE_TOOFAR;

    if ((nirBox->xmax - nirBox->xmin) > FACE_POSITION_BOUNDARY_CLOSE)
        return FACE_STATE_TOOCLOSE;

    if (nirBox->xmin < FACE_POSITION_BOUNDARY_HORIZONTAL)
        return FACE_STATE_TOOLEFT;

    if (nirBox->xmax > (1 - FACE_POSITION_BOUNDARY_HORIZONTAL))
        return FACE_STATE_TOORIGHT;

    if (nirBox->ymin < FACE_POSITION_BOUNDARY_VERTICAL)
        return FACE_STATE_TOOUP;

    if (nirBox->ymax > (1 - FACE_POSITION_BOUNDARY_VERTICAL))
        return FACE_STATE_TOODOWN;

    return FACE_STATE_NORMAL;
}

////////////////////////////func///////////////////////////////////
////////////////////////////func///////////////////////////////////
////////////////////////////func///////////////////////////////////

user_face_algo_sample_t *get_user_facec_algo_sample_info(void)
{
    if(!aos_mutex_is_valid(&g_userFaceAlgoSample.mutex))
	{
		aos_mutex_new(&g_userFaceAlgoSample.mutex);
	}
	return &g_userFaceAlgoSample;
}

user_face_manage_sample_t *get_user_facec_manage_sample_info(void)
{
	return &g_userFaceManageSample;
}

int multi_angle_result_send_user_sample(int result, int multiAngle, TsmFaceResult *faceResult)
{
    //result        本次角度检测成功与否 
    //                成功： 0
    //                失败： !0

    //multiAngle     本次检测的角度
                       

    //faceResult    算法结果信息
    return 0;
}

int user_multi_angle_reg_stop(void)
{
    g_ifv_flow_cfg.mid_face_reset_flag = 1;

    user_face_algo_sample_t *info = get_user_facec_algo_sample_info();
    if (0 == info->processRunFlag)
        return 0;
    else
        return 1;
    return 0;
}

int user_multi_angle_recog_sample_run(uint8_t timeout)
{
	struct timeval tv;
	
	user_face_algo_sample_t *info = get_user_facec_algo_sample_info();
	//如果线程正在执行检测，就返回
	if (1 == info->processRunFlag)
		return -1;

	aos_mutex_lock(&info->mutex, AOS_WAIT_FOREVER);

    //设置标志位
    info->processRunFlag = 1;

    //清除打断标志位
    g_ifv_flow_cfg.mid_face_reset_flag = 0;
    g_ifv_flow_cfg.mid_reset_flag = 0;

	//设置超时
	if(0 != timeout)
	{
		gettimeofday(&tv, NULL);
		info->recordTime = tv.tv_sec;
		info->timeout = timeout;
	}

	//通知算法运行
    multi_angle_recog_run();

	aos_mutex_unlock(&info->mutex);
	return 0;
}


int user_multi_angle_reg_sample_run(uint8_t admin, uint8_t *userName, uint8_t multiAngle, uint8_t timeout, uint16_t usrID)
{
	struct timeval tv;
	
	user_face_algo_sample_t *info = get_user_facec_algo_sample_info();
	user_face_manage_sample_t *manageInfo = get_user_facec_manage_sample_info();

	//如果线程正在执行检测，就返回
	if (1 == info->processRunFlag)
		return -1;

	aos_mutex_lock(&info->mutex, AOS_WAIT_FOREVER);

    //设置标志位
    info->processRunFlag = 1;
    info->coverRegisterFlag = 0;

    //清除打断标志位
    g_ifv_flow_cfg.mid_face_reset_flag = 0;
    g_ifv_flow_cfg.mid_reset_flag = 0;

    if(0xFFFF == usrID) {
        info->coverRegisterFlag = 0;
    } else if(usrID < USER_FACE_FEATURE_MAX_NUM){
         info->coverRegisterFlag = 1;
         info->usrID = usrID;
    } else {
        printf("usr id error! \n");
    }
    
	manageInfo->admin = admin;
	memcpy(manageInfo->userName, userName, USER_FACE_NAME_MAX_NUM);
		
	//设置超时	
	if(0 != timeout)
	{
		gettimeofday(&tv, NULL);
		info->recordTime = tv.tv_sec;
		info->timeout = timeout;
	}

    //设置本次检测的角度（左、右、上、下、中）
    if(g_ifv_flow_cfg.sensetime_protocol_enable)
    {
        info->multiAngle = FACE_DIRECTION_UNDEFINE;
        for(int i=0;i<FACE_DIRECTION_NUM_MAX;i++)
        {
            if((multiAngle >> i) == 0x01){
                info->multiAngle = multiAngle;
            }
        }

        if(info->multiAngle == FACE_DIRECTION_UNDEFINE){
            printf("direction error!!! return. \n");
            return 0;
        }
    }
    else
    {
        if((multiAngle >= 0) && (multiAngle < USER_MULTI_ANGLE_NUM))
		    info->multiAngle = multiAngle;
        else
        {
            info->recordTime = 0;
            info->timeout = 0;
            info->processRunFlag = 0;
            aos_mutex_unlock(&info->mutex);
            return -1;
        }
    }
		
	//通知算法运行
    multi_angle_reg_run();

	aos_mutex_unlock(&info->mutex);
	return 0;
}

int user_face_initial_sample(void)
{
	int ret = -1;
	user_face_algo_sample_t *algoInfo = get_user_facec_algo_sample_info();
	user_face_manage_sample_t *manageInfo = get_user_facec_manage_sample_info();

	memset(algoInfo, 0x0, sizeof(user_face_algo_sample_t));
	memset(manageInfo, 0x0, sizeof(user_face_manage_sample_t));

    g_userFaceFeature = (uint8_t*)aos_malloc(USER_FACE_FEATURE_MAX_NUM * USER_FACE_FEATURE_SIZE);
    if (NULL == g_userFaceFeature)
    {
        printf("Failed to allocate memory. %s %d\r\n", __func__, __LINE__);
		return -1;
    }

    g_userfaceFeatureTemp = (uint8_t*)aos_malloc(USER_MULTI_ANGLE_NUM * FACE_FEATURE_SIZE);
    if (NULL == g_userfaceFeatureTemp)
    {
        printf("Failed to allocate memory. %s %d\r\n", __func__, __LINE__);
    }
    
    ret = create_user_ff_dir_and_files();
    if (ret < 0)
    {
		printf("%s:%d error\n", __func__, __LINE__);
		return -1;
	}

	user_face_manage_ff_load();

	return 0;
}

void user_face_process_clear_sample(void)
{
	user_face_algo_sample_t *algoInfo = get_user_facec_algo_sample_info();
	memset(algoInfo, 0x0, sizeof(user_face_algo_sample_t));

	if(1 == g_ifv_flow_cfg.mid_reset_flag){
		g_ifv_flow_cfg.mid_reset_flag = 0;
		g_ifv_flow_cfg.mid_debug_mode_flag = 0;
		resetAck();
	}
	else if(1 == g_ifv_flow_cfg.mid_face_reset_flag){
		g_ifv_flow_cfg.mid_face_reset_flag = 0;
        g_ifv_flow_cfg.mid_debug_mode_flag = 0;
		faceResetAck();
	}
}

//放在回调中
int user_multi_angle_reg_result_check_sample(void *pData)
{
    int faceState = 0;
    struct timeval tv;
    TsmFaceResult *tp = (TsmFaceResult *)(((CmdRecognizeCBinfo_S *)pData)->tp);
	TsmFaceFeatrue *pFeature = &tp->feature;
	int tempId, coverRegRet,ret = -1;
	user_face_algo_sample_t *info = get_user_facec_algo_sample_info();
	user_face_manage_sample_t *manageInfo = get_user_facec_manage_sample_info();
    uint8_t *userfaceFeatureTemp = g_userfaceFeatureTemp;
	
	aos_mutex_lock(&info->mutex, AOS_WAIT_FOREVER);

    gettimeofday(&tv, NULL);

    //打断
    if((1 == g_ifv_flow_cfg.mid_face_reset_flag) || (1 == g_ifv_flow_cfg.mid_reset_flag))
    {
        ifv_printf(IFV_MSG_INFO, "%s:recv stop flag, stoping this face algo process\n", __func__);
        goto clear;
    }	

    //超时
    if(0 != info->timeout)
    {
        if(info->recordTime + info->timeout <= tv.tv_sec)
        {
            ifv_printf(IFV_MSG_INFO, "%s:timeout, stoping this face algo process\n", __func__);
			
			if(g_ifv_flow_cfg.sensetime_protocol_enable)
			{
				//ACK timeout
	            ret = MR_FAILED4_TIMEOUT;

                if(info->coverRegisterFlag)
    				multiAngleRegAck(MID_ENROLL_COVER, ret, 0xffff, info->multiAngle);
                else
                    multiAngleRegAck(MID_ENROLL, ret, 0xffff, info->multiAngle);
				aos_mutex_unlock(&info->mutex);
				info->processRunFlag = 0;
	    		return ret;
			}
			else
			{
				goto clear;
			}
        }
    }	

    if(g_ifv_flow_cfg.sensetime_protocol_enable)
    {
        if(1 == tp->nFace){
            faceState = multiAngleFacePositionCheck(tp->nirBox);
        }
        else if(tp->nFace > 1)
            faceState = 15;
        else
            faceState = FACE_STATE_NOFACE;

        if(FACE_STATE_NORMAL != faceState){
            ifv_printf(IFV_MSG_DEBUG, "#####################################################################\n");
            ifv_printf(IFV_MSG_INFO, "%s:check face position away from the center , will try again\n", __func__);
            ifv_printf(IFV_MSG_DEBUG, "#####################################################################\n");
            
            multiAngleRegNote(faceState,0,0,0,0,0,0,0);
    
            //通知算法继续运行
            multi_angle_reg_run();
            aos_mutex_unlock(&info->mutex);
            return -1;
        }
    }

	//算法结果判断
    if (0 == (((CmdRecognizeCBinfo_S *)pData)->result))
    {
        if(1 == tp->nFace)
        {
            ifv_printf(IFV_MSG_DEBUG, "pose[0]:%f pose[1]:%f\n", tp->qa[0].pose[0], tp->qa[0].pose[1]);
            ifv_printf(IFV_MSG_DEBUG,"angle:%d\n", info->multiAngle);
			
			if(g_ifv_flow_cfg.sensetime_protocol_enable)
			{
				//左 FACE_DIRECTION_LEFT
	            if((FACE_DIRECTION_LEFT == info->multiAngle) && (tp->qa[0].pose[0] > SIDE_FACE_ANGLE_MIN)
	                    && (tp->qa[0].pose[0] < SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:left\n", __func__);
					info->multiAngleReady |= FACE_DIRECTION_LEFT;
	                ret = 0;
					info->firstRegisterFlag++;
	            }
	            //右
	            else if((FACE_DIRECTION_RIGHT == info->multiAngle) && (tp->qa[0].pose[0] < -SIDE_FACE_ANGLE_MIN)
	                    && (tp->qa[0].pose[0] > -SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:right\n", __func__);
					info->multiAngleReady |= FACE_DIRECTION_RIGHT;
	                ret = 0;
					info->firstRegisterFlag++;
	            }
	            //抬头
	            else if((FACE_DIRECTION_UP == info->multiAngle) && (tp->qa[0].pose[1] > SIDE_FACE_ANGLE_MIN) 
	                    && (tp->qa[0].pose[1] < SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:up\n", __func__);
					info->multiAngleReady |= FACE_DIRECTION_UP;           
	                ret = 0;
					info->firstRegisterFlag++;
	            }
	            //低头
	            else if((FACE_DIRECTION_DOWN == info->multiAngle) && (tp->qa[0].pose[1] < -SIDE_FACE_ANGLE_MIN) 
	                    && (tp->qa[0].pose[1] > -SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:down\n", __func__);
					info->multiAngleReady |= FACE_DIRECTION_DOWN;
	                ret = 0;
					info->firstRegisterFlag++;
	            }
	            //正脸
	            else if((FACE_DIRECTION_MIDDLE == info->multiAngle) && (tp->qa[0].pose[0] > -FRONTAL_FACE_ANGLE) 
	                    && (tp->qa[0].pose[1] < FRONTAL_FACE_ANGLE) && (tp->qa[0].pose[1] > -FRONTAL_FACE_ANGLE) && (tp->qa[0].pose[1] < FRONTAL_FACE_ANGLE))
	            {
	                printf("%s:middle\n", __func__);
					info->multiAngleReady |= FACE_DIRECTION_MIDDLE;
	                ret = 0;
					info->firstRegisterFlag++;
	            }
				else
				{
					printf("%s:register success but angle is not match!\n", __func__);
					ret = FACE_STATE_DIRECTION_ERROR;
				}

				//first register,get user id 
				if(1 == info->firstRegisterFlag)
				{
                    if(!info->coverRegisterFlag)
                    {
                        tempId = user_face_manage_ff_get_usr_id();
                        if(tempId >= 0)
                        {
                            manageInfo->userId = tempId;
                        }
                    }
				}
			}
			else
			{
				//左
	            if((USER_FACE_CHECK_LEFT == info->multiAngle) && (tp->qa[0].pose[0] > SIDE_FACE_ANGLE_MIN) 
	                    && (tp->qa[0].pose[0] < SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:left\n", __func__);
					info->recordTime = tv.tv_sec;
	                ret = 0;
	            }
	            //右
	            else if((USER_FACE_CHECK_RIGHT == info->multiAngle) && (tp->qa[0].pose[0] < -SIDE_FACE_ANGLE_MIN) 
	                    && (tp->qa[0].pose[0] > -SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:right\n", __func__);
					info->recordTime = tv.tv_sec;
	                ret = 0;
	            }
	            //抬头
	            else if((USER_FACE_CHECK_UP == info->multiAngle) && (tp->qa[0].pose[1] > SIDE_FACE_ANGLE_MIN) 
	                    && (tp->qa[0].pose[1] < SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:up\n", __func__);
					info->recordTime = tv.tv_sec;                
	                ret = 0;
	            }
	            //低头
	            else if((USER_FACE_CHECK_DOWN == info->multiAngle) && (tp->qa[0].pose[1] < -SIDE_FACE_ANGLE_MIN) 
	                    && (tp->qa[0].pose[1] > -SIDE_FACE_ANGLE_MAX))
	            {
	                printf("%s:down\n", __func__);
					info->recordTime = tv.tv_sec;
	                ret = 0;
	            }
	            //正脸
	            else if((USER_FACE_CHECK_MIDDLE == info->multiAngle) && (tp->qa[0].pose[0] > -FRONTAL_FACE_ANGLE) 
	                    && (tp->qa[0].pose[0] < FRONTAL_FACE_ANGLE) && (tp->qa[0].pose[1] > -FRONTAL_FACE_ANGLE) && (tp->qa[0].pose[1] < FRONTAL_FACE_ANGLE))
	            {
	                printf("%s:middle\n", __func__);
					info->recordTime = tv.tv_sec;
	                ret = 0;
	            }
			}
        }
        else
        {           
            printf("face num is not 1:%d\n", tp->nFace);
        }
    }
    else
    {
        printf("%s:%d:result=%d\n", __func__, __LINE__, (((CmdRecognizeCBinfo_S *)pData)->result));
		//NOTE : result
		ret = ((CmdRecognizeCBinfo_S *)pData)->result;
    }

    //将结果返回给用户
    multi_angle_result_send_user_sample(ret, info->multiAngle, tp);

    if(0 == ret)
    {
        ifv_printf(IFV_MSG_DEBUG, "#####################################################################\n");
        ifv_printf(IFV_MSG_INFO, "%s:check multi angle :[%d] success, please input the next angle\n", __func__, info->multiAngle);
        ifv_printf(IFV_MSG_DEBUG, "#####################################################################\n");
        
		if(g_ifv_flow_cfg.sensetime_protocol_enable)
		{
            for(int i=0;i<FACE_DIRECTION_NUM_MAX;i++)
            {
                if((info->multiAngle >> i) == 0x01)
                {
                    //将该角度的特征信息保存在缓存中
                    memset(userfaceFeatureTemp + i * FACE_FEATURE_SIZE, 0xff, FACE_FEATURE_SIZE);
                    memcpy(userfaceFeatureTemp + i * FACE_FEATURE_SIZE, pFeature->vector, FACE_FEATURE_SIZE);
                    break;
                }
            }

			//success ACK
            if(info->coverRegisterFlag){
                multiAngleRegAck(MID_ENROLL_COVER, ret, info->usrID, info->multiAngleReady);
            }else{
                multiAngleRegAck(MID_ENROLL, ret, manageInfo->userId, info->multiAngleReady);
            }

            info->processRunFlag = 0;

            if(FACE_DIRECTION_ALL == info->multiAngleReady)
            {
                printf("%s:multi angle register over!\n", __func__);

                if(info->coverRegisterFlag)
                {
                    coverRegRet = user_face_manage_ff_set_usr_id(info->usrID);
                    if(0 == coverRegRet)
                        manageInfo->userId = info->usrID;
                    else if(1 == coverRegRet){
                        printf("user id exist, del current id by usr id \n");
                        user_face_manage_ff_delete(info->usrID);
                        manageInfo->userId = info->usrID;
                    }
                    else
                        printf("set_usr_id para err! \n");
                }
                    
                //save feature
                user_face_manage_ff_add(manageInfo->userId);
                aos_msleep(10);
                goto clear;
            }
		}
		else
		{
             //将该角度的特征信息保存在缓存中
            memset(userfaceFeatureTemp + info->multiAngle * FACE_FEATURE_SIZE, 0x0, FACE_FEATURE_SIZE);
            memcpy(userfaceFeatureTemp + info->multiAngle * FACE_FEATURE_SIZE, pFeature->vector, FACE_FEATURE_SIZE);
			goto clear;
		}
    }
    else if(FACE_RESULT_REPEAT == ret)
    {
        printf("repeat register \n");
        ret = MR_FAILED4_FACEENROLLED;

        if(info->coverRegisterFlag)
            multiAngleRegAck(MID_ENROLL_COVER, ret, 0xffff, info->multiAngle);
        else
            multiAngleRegAck(MID_ENROLL, ret, 0xffff, info->multiAngle);

        goto clear;
    }
    else
    {
        ifv_printf(IFV_MSG_DEBUG,"#####################################################################\n");
        ifv_printf(IFV_MSG_INFO,"%s:check multi angle :[%d] failed, will try again\n", __func__, info->multiAngle);
        ifv_printf(IFV_MSG_DEBUG,"#####################################################################\n");

        if(g_ifv_flow_cfg.sensetime_protocol_enable)
        {
            int16_t roll = (int16_t)(round(tp->qa[0].pose[0]));
            int16_t pitch = (int16_t)(round(tp->qa[0].pose[1]));
            int16_t yaw = (int16_t)(round(tp->qa[0].pose[2]));

            //failed NOTE
            multiAngleRegNote(faceState,0,0,0,0,yaw,pitch,roll);
        }

        //通知算法继续运行
        multi_angle_reg_run();
    }

    aos_mutex_unlock(&info->mutex);
    return ret;

clear:
    aos_mutex_unlock(&info->mutex);
	printf("%s:%d exit \n", __func__, __LINE__);
	user_face_process_clear_sample();            
	
	return ret;
}

//返回用户二次管理的id
int user_multi_angle_recog_result_check_sample(void *pData)
{
    int32_t id = ((CmdRecognizeCBinfo_S *)pData)->id;
    int i = 0,j = 0;
	int ret = -1;
    user_face_manage_sample_t *manageInfo = NULL;
	TsmFaceResult *tp = (TsmFaceResult *)(((CmdRecognizeCBinfo_S *)pData)->tp);
	user_face_algo_sample_t *info = get_user_facec_algo_sample_info();
	struct timeval tv;
    int faceState = 0;

	aos_mutex_lock(&info->mutex, AOS_WAIT_FOREVER);

    gettimeofday(&tv, NULL);

    //打断
    if((1 == g_ifv_flow_cfg.mid_face_reset_flag) || (1 == g_ifv_flow_cfg.mid_reset_flag))
    {
        printf("%s:recv stop flag, stoping this face algo process\n", __func__);
        goto clear;
    }	

	//超时
    if(0 != info->timeout)
    {
        if(info->recordTime + info->timeout <= tv.tv_sec)
        {
            printf("%s:timeout, stoping this face algo process\n", __func__);
            //ACK timeout
            ret = MR_FAILED4_TIMEOUT;
			multiAngleRecogAck(ret, 0xffff, NULL, 0, 0);
			user_face_process_clear_sample();
			aos_mutex_unlock(&info->mutex);
    		return ret;
        }
    }

    for(; i < USER_FACE_FEATURE_MAX_NUM; i++)
    {
    	if(false == g_userFfMap[i])
			continue;
        manageInfo = (user_face_manage_sample_t *)(g_userFaceFeature + i * USER_FACE_FEATURE_SIZE);
        for(j = 0; j < USER_MULTI_ANGLE_NUM; j++)
        {
            if(manageInfo->multiAngleIdMap[j] == id){
				ret = 0;
				break;
			}    
        }

		if(0 == ret)
			break;
    }

	if((0 == ret) && (!g_ifv_flow_cfg.mid_debug_mode_flag)){
        ifv_printf(IFV_MSG_DEBUG,"#####################################################################\n");
        ifv_printf(IFV_MSG_INFO,"                      recog user id: %d \n", manageInfo->userId);
        ifv_printf(IFV_MSG_DEBUG,"#####################################################################\n");
		
		multiAngleRecogAck(((CmdRecognizeCBinfo_S *)pData)->result, manageInfo->userId, manageInfo->userName, manageInfo->admin, 0);
		user_face_process_clear_sample();
	}	
	else{
		int16_t roll = (int16_t)(round(tp->qa[0].pose[0]));
		int16_t pitch = (int16_t)(round(tp->qa[0].pose[1]));
		int16_t yaw = (int16_t)(round(tp->qa[0].pose[2]));

		if(g_ifv_flow_cfg.mid_debug_mode_flag){
			int result = ((CmdRecognizeCBinfo_S *)pData)->result;
			if((0 == result) || (FACE_RESULT_NOT_MATCH == result)){
				uint8_t name[32] = {0};
				multiAngleRecogAck(0, 0, name, 0, 0);
			}
		}
		else{
			//failed NOTE
			multiAngleRecogNote(faceState,0,0,0,0,yaw,pitch,roll);
		}
		
		if(g_ifv_flow_cfg.sensetime_protocol_enable){
			//通知算法继续运行
			multi_angle_recog_run();	
		}
		else
		{
			 goto clear;
		}		
	}

	aos_mutex_unlock(&info->mutex);
	return ret;
	
clear:
	aos_mutex_unlock(&info->mutex);
	printf("%s:%d enter\n", __func__, __LINE__);
	user_face_process_clear_sample();	 

    return -1;
}

void user_multi_angle_result_check_sample(void *pData)
{
	int ret = -1;
    user_face_algo_sample_t *info = get_user_facec_algo_sample_info();

	//多角度注册判断
	if((1 == info->processRunFlag) && (1 == (((CmdRecognizeCBinfo_S *)pData)->flag)))
	{
		//printf("%s:%d\n", __func__, __LINE__);
		user_multi_angle_reg_result_check_sample(pData);
	}
	//用户二次管理识别结果返回
	if(2 == ((CmdRecognizeCBinfo_S *)pData)->flag)
	{
		//printf("%s:%d\n", __func__, __LINE__);
		ret = user_multi_angle_recog_result_check_sample(pData);
	}

    return;
}

/////////////////////////////////////////////////////////face feature manage func////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////face feature manage func////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////face feature manage func////////////////////////////////////////////////////////////////
int user_face_manage_ff_get_usr_id(void)
{
	int32_t id, found_slot = false;
	
	// 1. Find a extra slot for face feature
    for (id = 0; id < USER_FACE_FEATURE_MAX_NUM; id++)
    {
        if (false == g_userFfMap[id])
        {
            found_slot = true;
            break;
        }
    }
    if (false == found_slot)
    {
		printf("%s:%d error\n", __func__, __LINE__);
        return -1;
    }

	return id;
}

/*
-1: failed
0: success
1: repet set
*/
int user_face_manage_ff_set_usr_id(int id)
{
	// int32_t found_slot = false;
	
    if((id < 0) || (id > USER_FACE_FEATURE_MAX_NUM))
        return -1;

    if (true == g_userFfMap[id]){
         return 1;
    }else
    {
        g_userFfMap[id] = true;
    }
       
	return 0;
}

/*
 *  Face feature add function
 *  @return    ID of face feature
 */
int user_face_manage_ff_add(uint16_t usr_id)
{
    int32_t retval =0;
	int32_t tsmId = 0;
	// uint8_t ff_id_str[16] = {0};
	uint8_t *userfaceFeatureTemp = g_userfaceFeatureTemp;
	user_face_manage_sample_t *manageInfo = get_user_facec_manage_sample_info();
    int32_t m, n = 0;
    int32_t userId = -1;
	
	if(g_ifv_flow_cfg.sensetime_protocol_enable)
	{
		manageInfo->userId = usr_id;
	}
	else
	{
		manageInfo->userId = user_face_manage_ff_get_usr_id();
	}

	//原始底层特征保存
#if FIXED_LENGTH_SWITCH
    tsmId = tsm_ff_add_fixed_length((void *)(userfaceFeatureTemp), NULL, USER_MULTI_ANGLE_NUM);
    if (tsmId < 0)
    {
        printf("%s:%d error\n", __func__, __LINE__);
        goto exit;
    }

    for(m = 0;m < USER_MULTI_ANGLE_NUM; m++)
	{
		manageInfo->multiAngleIdMap[m] = tsmId + m;
        printf("%s:multiAngleIdMap[%d]=%d\n", __func__, m, tsmId + m);
    }
#else
	for(m = 0;m < USER_MULTI_ANGLE_NUM; m++)
	{
        // TsmFaceFeatrue
        // ff_id = tsm_ff_add(pFeature->vector + pFeature->len * 0, pMaskFeature->vector + pMaskFeature->len * 0);
        tsmId = tsm_ff_add((void *)(userfaceFeatureTemp + FACE_FEATURE_SIZE * m), NULL);
		if (tsmId < 0)
		{
			printf("%s:%d error\n", __func__, __LINE__);
			goto exit;
		}

		manageInfo->multiAngleIdMap[m] = tsmId;
        printf("%s:multiAngleIdMap[%d]=%d\n", __func__, m, tsmId);
	}
#endif
	
	// 2. Write face feature to memory
    memcpy(g_userFaceFeature + manageInfo->userId * USER_FACE_FEATURE_SIZE, manageInfo, USER_FACE_FEATURE_SIZE);

#ifdef AOS_CONFIG_FF_USE_KV
    // 3. Write face feature to flash
    itoa(manageInfo->userId, ff_id_str);
    retval = aos_kv_set(ff_id_str, manageInfo, USER_FACE_FEATURE_SIZE, 1);
    if (retval != 0) 
    {
		printf("%s:%d error\n", __func__, __LINE__);
        goto exit;
    }
#elif defined(AOS_CONFIG_FF_USE_FS)
    retval = user_ff_add_to_fs(manageInfo->userId, manageInfo, true);
    if (retval < 0)
	{
		printf("%s:%d error\n", __func__, __LINE__);
        goto exit;
    }
#endif    
    g_userFfMap[manageInfo->userId] = true;

    userId = manageInfo->userId;
    memset(manageInfo, 0x0, sizeof(user_face_manage_sample_t));

	return userId;

exit:
	//清理底层人脸特征
#if FIXED_LENGTH_SWITCH
    retval = tsm_ff_delete_fixed_length(manageInfo->multiAngleIdMap[0], USER_MULTI_ANGLE_NUM);
    if (retval < 0)
    {
        printf("%s:%d error\n", __func__, __LINE__);
		return -1;
    }
#else
	for(n = 0; n < m; n++)
	{
		retval = tsm_ff_delete(manageInfo->multiAngleIdMap[n]);
		if (retval < 0)
		{
			printf("%s:%d error\n", __func__, __LINE__);
			return -1;
		}
	}
#endif

    memset(manageInfo, 0x0, sizeof(user_face_manage_sample_t));

	return -1;
}

/*  
 *  Face feature delete function
 *  @param[in] id          ID of face feature.
 *  @return    0 success other failed
 */
int user_face_manage_ff_delete(uint32_t id)
{
    int32_t retval = 0;
	// uint8_t ff_id_str[16] = {0};
	user_face_manage_sample_t *manageInfo = NULL;
    int32_t i = 0;

    if (id >= USER_FACE_FEATURE_MAX_NUM)
    {
        printf("%s:id:[%d] is not in list \n", __func__, id);
        return -1;
    }

    if(false == g_userFfMap[id])
    {
        printf("%s:id:[%d] is not exist\n", __func__, id);
        return 0;
    }

    manageInfo = (user_face_manage_sample_t *)(g_userFaceFeature + id * USER_FACE_FEATURE_SIZE);

	//清理底层人脸特征
#if FIXED_LENGTH_SWITCH
    retval = tsm_ff_delete_fixed_length(manageInfo->multiAngleIdMap[0], USER_MULTI_ANGLE_NUM);
    if (retval < 0)
    {
        printf("%s:%d error\n", __func__, __LINE__);
		return -1;
    }
#else
	for(i = 0; i < USER_MULTI_ANGLE_NUM; i++)
	{
		retval = tsm_ff_delete(manageInfo->multiAngleIdMap[i]);
		if (retval < 0)
		{
			printf("%s:%d error\n", __func__, __LINE__);
		}
	}
#endif

	// 1. Zero face feature zone in memory
    memset(g_userFaceFeature + id * USER_FACE_FEATURE_SIZE, 0xff, USER_FACE_FEATURE_SIZE);

#ifdef AOS_CONFIG_FF_USE_KV
    // 2. Delete face feature in flash
    itoa(id, ff_id_str);
    retval = aos_kv_del(ff_id_str);
#elif defined(AOS_CONFIG_FF_USE_FS)
    retval = user_ff_delete_from_fs(id, false);
    if (retval < 0) {
        printf("delete ff from fs error.\n");
        return retval;
    }
#endif
    // 3. Clear flag for this face feature
    g_userFfMap[id] = false;

    return retval;
}

/*  
 *  Face feature clear function
 *  @return    0 success other failed
 */
int user_face_manage_ff_clear(void)
{
    int32_t retval = 0;
    
	//清理底层人脸特征
	retval = tsm_ff_clear();
	if (retval < 0)
	{
		printf("%s:%d error\n", __func__, __LINE__);
	}

    // 1. Zero face feature zone in memory
    memset(g_userFaceFeature, 0xff, USER_FACE_FEATURE_MAX_NUM * USER_FACE_FEATURE_SIZE);
#ifdef AOS_CONFIG_FF_USE_KV   
	uint8_t ff_id_str[16] = {0};
    for (uint32_t id = 0; id < USER_FACE_FEATURE_MAX_NUM; id++)
    {
        if (true == g_userFfMap[id])
        {
            // 2. Delete face feature in flash
            itoa(id, ff_id_str);
            retval = aos_kv_del(ff_id_str);
            if (retval < 0)
            {
                return retval;
            }
            
            // 3. Clear flag for this face feature
            g_userFfMap[id] = false;
            //if_vDelRgbFile(id);
        }
    }
#elif defined(AOS_CONFIG_FF_USE_FS)    
    memset(g_userFfMap, 0x0, sizeof(g_userFfMap));
    retval = user_ff_all_delete_from_fs((char *)g_userFfMap, sizeof(g_userFfMap));
    if (retval < 0) {
        printf("delete all ff from fs error.\n");
    }
#endif
    return retval;
}

/*  
 *  Face feature initialization function
 *  @param[in] 
 *  @return    0 success other failed
 */
int user_face_manage_ff_load(void)
{
    int ret = 0;

    memset((uint8_t *)g_userFaceFeature, 0xff, USER_FACE_FEATURE_MAX_NUM * USER_FACE_FEATURE_SIZE);
    
#ifdef AOS_CONFIG_FF_USE_KV    
    for (uint32_t i = 0; i < KV_BLOCK_NUMS; i++) 
    {
        kv_item_traverse(__ff_load_cb, i, NULL);
    }
#elif defined(AOS_CONFIG_FF_USE_FS)
	//ret = ff_load_from_fs();
    ret = user_ff_load_from_fs();
#endif
    
    return ret;
}

/*  
 *  Face feature get function
 *  @param[in] id          ID of face feature.
 *  @param[in] feature    pointer to the face feature.
 */
int user_face_manage_ff_get(uint32_t id, void *feature)
{
    memcpy(feature, g_userFaceFeature + id * USER_FACE_FEATURE_SIZE, USER_FACE_FEATURE_SIZE);
    return 0;
}

int user_face_manage_ff_num(void)
{
	uint32_t id = 0, num = 0;
    for (; id < USER_FACE_FEATURE_MAX_NUM; id++)
    {
        if (true == g_userFfMap[id])
        {
            num++;
        }
    }

    return num;
}

void user_face_manage_ff_list(uint16_t *list)
{
	uint16_t id = 0;

	if(NULL == list)
		return;
	
    for (; id < USER_FACE_FEATURE_MAX_NUM; id++)
    {
        if (true == g_userFfMap[id])
        {
            *list++ = id;
			printf("user ID: %d \n", id);
        }
    }
}


int user_face_get_idle_id(void)
{
	int32_t id, found_slot = false;

    for (id = 0; id < USER_FACE_FEATURE_MAX_NUM; id++)
    {
        if (false == g_userFfMap[id])
        {
            found_slot = true;
            break;
        }
    }
    if (false == found_slot)
    {
		printf("%s:%d error\n", __func__, __LINE__);
        return -1;
    }
    return 0;
}

////////////////////////////////////////////////////////////用户相关接口////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////用户相关接口////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////用户相关接口////////////////////////////////////////////////////////////
int get_user_info_by_id(uint16_t user_id, uint8_t *name, uint8_t *admin)
{
	int i = 0;
	int ret = -1;
    user_face_manage_sample_t *manageInfo = NULL;

	if((NULL == name) || (NULL == admin))
		return ret;

	if(false == g_userFfMap[user_id])
		return ret;

    for(; i < USER_FACE_FEATURE_MAX_NUM; i++)
    {
    	if(false == g_userFfMap[i])
			continue;
		
        manageInfo = (user_face_manage_sample_t *)(g_userFaceFeature + i * USER_FACE_FEATURE_SIZE);
        if(manageInfo->userId == user_id){
			memcpy(name, manageInfo->userName, USER_FACE_NAME_MAX_NUM);
			*admin = manageInfo->admin;
			ret = 0;
			break;
		}    
    }

	return ret;
}

int get_all_user_info(uint8_t *name, uint8_t *admin)
{
	int ret = -1;
	
	return ret;
}


////////////////////////////////////////////////////////////以下内容为辅助实现，用户不必关心////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////以下内容为辅助实现，用户不必关心////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////以下内容为辅助实现，用户不必关心////////////////////////////////////////////////////////////

typedef enum ff_err_status {
    FF_OPEN_FILE_ERR   = -1,
    FF_READ_FILE_ERR   = -2,
    FF_WRITE_FILE_ERR  = -3,
    FF_SEEK_FILE_ERR   = -4,
    FF_OPT_SUCCESS     = 0,
} ff_err_status_t;

#define FF_DIR_PATH FS_ROOT_PATH"/ff"
#define USER_FF_FILE_PATH FF_DIR_PATH"/userff"
#define USER_FF_INFO_PATH FF_DIR_PATH"/userffinfo"
#define USER_FF_ID2NAME_PATH FF_DIR_PATH"/userffid2name"

static int user_ff_add_to_fs(uint32_t id, void *pFeature, char flag)
{
    int ret = FF_OPT_SUCCESS;
    int ff = 0;
    int ffinfo = 0;

    ffinfo = aos_open(USER_FF_INFO_PATH, O_RDWR);
    if (ffinfo < 0) {
        printf("ffinfo file not exit(%d)\n", ffinfo);
        ret = FF_OPEN_FILE_ERR;
        goto exit;
    }
    ff = aos_open(USER_FF_FILE_PATH, O_RDWR);
    if (ff < 0) {
        printf("ff file not exit(%d)\n", ff);
        ret = FF_OPEN_FILE_ERR;
        goto exit;
    }
    ret = aos_lseek(ff, id * USER_FACE_FEATURE_SIZE, SEEK_SET);
    if (ret < 0) {
        printf("mv ff file pos error.\n");
        ret = FF_SEEK_FILE_ERR;
        goto exit;
    }
    ret = aos_write(ff, pFeature, USER_FACE_FEATURE_SIZE);
    if (ret < 0 || ret != USER_FACE_FEATURE_SIZE) {
        printf("write ff file error.\n");
        ret = FF_WRITE_FILE_ERR;
        goto exit;
    }
    ret = aos_lseek(ffinfo, id *sizeof(char), SEEK_SET);
    if (ret < 0) {
        printf("mv ffinfo file pos error.\n");
        ret = FF_SEEK_FILE_ERR;
        goto exit;
    }
    ret = aos_write(ffinfo, &flag, sizeof(char));
    if (ret < 0 || ret != sizeof(char)) {
        printf("write ffinfo file error.\n");
        ret = FF_WRITE_FILE_ERR;
        goto exit;
    }

exit:
    if (ff) {
        aos_close(ff);
    }
    if (ffinfo) {
        aos_close(ffinfo);
    }

    return ret;
}

static int user_ff_delete_from_fs(uint32_t id, char flag)
{
    int ret = FF_OPT_SUCCESS;
    int ffinfo = 0;

    ffinfo = aos_open(USER_FF_INFO_PATH, O_RDWR);
    if (ffinfo < 0) {
        printf("ff info file not exit\n");
        ret = FF_OPEN_FILE_ERR;
        goto exit;
    }
    ret = aos_lseek(ffinfo, id *sizeof(flag), SEEK_SET);
    if (ret < 0) {
        printf("mv ffinfo file pos error.\n");
        ret = FF_SEEK_FILE_ERR;
        goto exit;
    }
    ret = aos_write(ffinfo, &flag, sizeof(flag));
    if (ret < 0 || ret != sizeof(flag)) {
        printf("write ffinfo file error.\n");
        ret = FF_WRITE_FILE_ERR;
        goto exit;
    }

exit:
    if (ffinfo) {
        aos_close(ffinfo);
    }

    return ret;
}

static int user_ff_all_delete_from_fs(char *buf, uint32_t len)
{
    int ret = FF_OPT_SUCCESS;
    int ffinfo = 0;

    ffinfo = aos_open(USER_FF_INFO_PATH, O_RDWR);
    if (ffinfo < 0) {
        printf("ff info file not exit\n");
        ret = FF_OPEN_FILE_ERR;
        goto exit;
    }
    ret = aos_write(ffinfo, buf, len);
    if (ret < 0 || ret != len) {
        printf("read ff file error.\n");
        ret = FF_WRITE_FILE_ERR;
        goto exit;
    }

exit:
    if (ffinfo) {
        aos_close(ffinfo);
    }

    return ret;
}

static int user_ff_load_from_fs(void)
{
    int ret = FF_OPT_SUCCESS;
    int ff = 0;
    int ffinfo = 0;

    ffinfo = aos_open(USER_FF_INFO_PATH, O_RDONLY);
    if (ffinfo < 0) {
        printf("ffinfo file not exit\n");
        ret = FF_OPEN_FILE_ERR;
        goto exit;
    }
    ff = aos_open(USER_FF_FILE_PATH, O_RDONLY);
    if (ff < 0) {
        printf("ff file not exit\n");
        ret = FF_OPEN_FILE_ERR;
        goto exit;
    }
    ret = aos_read(ffinfo, g_userFfMap, sizeof(g_userFfMap));
    if (ret < 0 || ret != sizeof(g_userFfMap)) {
        printf("read ffinfo error, ret %d\r\n", ret);
        ret = FF_READ_FILE_ERR;
        goto exit;
    }
    for (int id = 0; id < USER_FACE_FEATURE_MAX_NUM; id++) {
        if (g_userFfMap[id] == true) {
            ret = aos_lseek(ff, id * USER_FACE_FEATURE_SIZE, SEEK_SET);
            if (ret < 0) {
                printf("mv ff file pos err, maybe ff don't match to ffinfo.\n");
                ret = FF_SEEK_FILE_ERR;
                goto exit;
            }
            ret = aos_read(ff, (uint8_t *)(g_userFaceFeature) + id * USER_FACE_FEATURE_SIZE, USER_FACE_FEATURE_SIZE);
            if (ret < 0 || ret != USER_FACE_FEATURE_SIZE) {
                printf("read ff file feature error, ret %d.\n", ret);
                ret = FF_READ_FILE_ERR;
                goto exit;
            }
        } 
    }
exit:
    if (ff) {
        aos_close(ff);
    }
    if (ffinfo) {
        aos_close(ffinfo);
    }

    return ret;
}

int user_ff_dump_id(void)
{
	uint32_t id = 0;
    cli_printf("Face feature ID list: \r\n[");
    for (; id < USER_FACE_FEATURE_MAX_NUM; id++)
    {
        if (true == g_userFfMap[id])
        {
            cli_printf(" %d ", id);
        }
    }
    cli_printf("]\r\n");
    return 0;
}

static int is_path_dir(const char *path)
{
    int ret = 0;
    aos_dir_t *dp = aos_opendir(path);

    if (dp) {
        ret = 1;
        aos_closedir(dp);
    }
    //printf("ret %d\n",ret);
    return ret;
}

static int check_or_create_dir(const char *path)
{
    int ret;
    
    if(is_path_dir(path)){
        return 0;
    }

    ret = aos_mkdir(path);

    if(ret < 0){
        printf("aos_mkdir [%s] ret=%d\n", path, ret);
        return -1;
    }

    return is_path_dir(path) ? 0 : -1;
}

static int is_file_exit(const char *path)
{
    int ret = 0;
    int fd = aos_open(path, O_RDONLY);

    if (fd >= 0) {
        ret = 1;
        aos_close(fd);
    }

    return ret;
}

static int check_or_create_file(const char *path)
{
    int fd = 0;
    int ret = 0;
    
    if(is_file_exit(path)){
        return 0;
    } 
    printf("create or re-create %s file.\n", path);
    fd = aos_open(path, O_RDWR | O_CREAT);
    if(fd < 0){
        printf("aos_open [%s] ret=%d\n", path, fd);
        ret = -1;
        goto exit;
    }
    memset(g_userFfMap, 0x0, sizeof(g_userFfMap));
    if(aos_write(fd, g_userFfMap, sizeof(g_userFfMap)) != sizeof(g_userFfMap)) {
        ret = -1;
        printf("init ffinfo write to file error.\n");
    }
    if (fd) {
        aos_close(fd);
    }
exit:
    return is_file_exit(path) ? ret : -1;
}

static int create_user_ff_dir_and_files(void)
{
   if (check_or_create_dir(FF_DIR_PATH) < 0){
        printf("FaceFeature dir not exist.\n");
        return -1;
    }
    if (check_or_create_file(USER_FF_FILE_PATH) < 0) {
        printf("%s:file not exist.\n", USER_FF_FILE_PATH);
        return -1;
    }
    if (check_or_create_file(USER_FF_INFO_PATH) < 0) {
        printf("%s:file not exist.\n", USER_FF_INFO_PATH);
        return -1;
    }

    if (check_or_create_file(USER_FF_ID2NAME_PATH) < 0) {
        printf("%s:file not exist.\n", USER_FF_ID2NAME_PATH);
        return -1;
    }

    return 0;
} 

